﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using NGDTConsole.Data.Parameters;

namespace NGDTConsole.CoordinateSystem
{
    public class CoordinateConversion
    {        

        public CartesianCoordinate CartesianCoordinateConversion(GeodeticCoordinate point, Ellipsoid ellipsoid, int precision = 0)
        {            
            double a = ellipsoid.MajorAxis;
            double e = ellipsoid.Eccentricity;
            
            double v = ellipsoid.PrimeVertical;

            double x = (v + point.Height) * (Math.Cos(point.Longitude.LongitudeRadian) * Math.Cos(point.Latitude.LatitudeRadian));
            double y = (v + point.Height) * (Math.Cos(point.Latitude.LatitudeRadian) * Math.Sin(point.Longitude.LongitudeRadian));
            double z = (v * (1 - e) + point.Height) * (Math.Sin(point.Latitude.LatitudeRadian));
            
            CartesianCoordinate coordinate = new CartesianCoordinate();
            coordinate.CartesianX = Math.Round(x, precision);
            coordinate.CartesianY = Math.Round(y, precision);
            coordinate.CartesianZ = Math.Round(z, precision);
            
            return coordinate;
        }

        public IList<CartesianCoordinate> CartesianCoordinateConversion(IList<GeodeticCoordinate> points, Ellipsoid ellipsoid, int precision = 0)
        {
            IList<CartesianCoordinate> coordinates = new List<CartesianCoordinate>();
            foreach (GeodeticCoordinate p in points)
            {
                coordinates.Add(CartesianCoordinateConversion(p, ellipsoid, precision));
            }
            return coordinates;
        }

        public GeodeticCoordinate CurvilinearCoordinateConversion(CartesianCoordinate point, Ellipsoid ellipsoid, int precision = 0)
        {
            //get the position
            double X = point.CartesianX;
            double Y = point.CartesianY;
            double Z = point.CartesianZ;

            double latitude = 0.0;
            double longitude = 0.0;
            double height = 0.0;

            double a = ellipsoid.MajorAxis;
            double b = ellipsoid.MinorAxis;

            double P = Math.Sqrt(Math.Pow(X, 2) + Math.Pow(Y, 2));

            double u = Math.Atan((a * Z) / (b * P));

            double ep = ellipsoid.SecondEccentricity;
            double e = ellipsoid.Eccentricity;

            latitude = Math.Atan( (Z + (ep * b * Math.Pow(Math.Sin(u), 3))) / (P - e*a*(Math.Pow(Math.Cos(u), 3))) );

            longitude = Math.Atan(Y / X);

            height = P * Math.Cos(latitude) + Z * Math.Sin(latitude) - a * Math.Sqrt(1 - (e * Math.Pow(Math.Sin(latitude), 2)));

            GeodeticCoordinate coordinate = new GeodeticCoordinate();
            coordinate.Latitude = new Latitude(latitude, Data.Enumerations.GeodeticMeasurementUnits.Radian);
            coordinate.Longitude = new Longitude(longitude, Data.Enumerations.GeodeticMeasurementUnits.Radian);
            coordinate.Height = height;
            return coordinate;
        }

        public IList<GeodeticCoordinate> CurvilinearCoordinateConversion(IList<CartesianCoordinate> points, Ellipsoid ellipsoid, int precision = 0)
        {
            IList<GeodeticCoordinate> coordinates = new List<GeodeticCoordinate>();
            foreach (CartesianCoordinate cc in points)
            {
                GeodeticCoordinate gpoint = CurvilinearCoordinateConversion(cc, ellipsoid, 0);
                coordinates.Add(gpoint);
            }
            return coordinates;
        }
    }
}
